#version 420 core

#define POSITION	0
#define VELOCITY	1
#define VERTEX		2
#define NORMAL	 	3
#define TEXCOORD 	4
#define FRAG_COLOR	0

layout(location = POSITION) in vec4 PositionMass;
layout(location = VELOCITY) in vec4 VelocityRadius;

uniform samplerBuffer PositionsMasses;
uniform samplerBuffer VelocitiesRadiuses;
uniform samplerBuffer CollisionState;

uniform int particle_count;
uniform float dt;
uniform vec3 planeNormal;
uniform vec3 planeDistance;

out block
{
	vec3 Position_out;
	vec3 Velocity_out;
} Out;

//Perform some kind of lookup in a state TBO to check if
//collisions already has been performed between these two
//particles...
bool alreadyChecked(in int iA, in int iB)
{
	//This approach would only support state storage
	//of 4 values per index... probably not useful :P
	if(	(int(CollisionState[iB].x) == iA) ||
		(int(CollisionState[iB].y) == iA) ||
		(int(CollisionState[iB].z) == iA) ||
		(int(CollisionState[iB].w) == iA) )
		return true;
	return false;
}

void swap(inout float a, inout float b)
{
	float tmp = a;
	a = b;
	b = tmp;
}

bool quadraticFormula(in float a, in float b, in float c, out float r1, out float r2)
{
	float q = b*b-4*a*c;
	if(q >= 0)
	{
		float sq = sqrt(q);
		float d = 1 / (2*a);
		r1 = (-b + sq) * d;
		r2 = (-b - sq) * d;
		
		return true; // real roots
	}
	
	return false; // complex roots
}

//Test intersection of particle with some ground plane...
bool spherePlaneSweep(in float r, in vec3 C0, in vec3 C1, in vec3 planeN, in vec3 planeD, out vec3 Ci, out float u)
{
	float d0 = dot(planeN, C0) + planeD;
	float d1 = dot(planeN, C1) + planeD;
	
	//check if it was touching on previous frame
	if(abs(d0) <= r)
	{
		Ci = C0;
		u = 0;
		return true;
	}
	
	//check if the sphere penetrated during this frame
	if( d0 > r && d1 < r )
	{
		u = (d0-r)/(d0-d1); //normalized time
		Ci = (1-u)*C0 + u*C1; //Point of first contact
		return true;
	}
	
	return false;
}

//Test intersection of particle with particle
bool sphereSphereSweep(	in float rA, in vec3 A0, in vec3 A1, 
						in float rB, in vec3 B0, in vec3 B1,
						out float u0, out float u1 )
{
	vec3 vA = A1-A0;
	vec3 vB = B1-B0;
	vec3 AB = B0-A0;
	vec3 vAB = vB-vA;
	float rAB = rA+rB;
	float A = dot(vAB, vAB);
	float B = 2 * dot(vAB, AB);
	float C = dot(AB,AB) - rAB*rAB;
	
	//Check if spheres currently overlap
	if( dot(AB,AB) <= rAB*rAB )
	{
		u0 = 0;
		u1 = 0;
		return true;
	}
	
	//Check if they hit each other during the frame
	if( quadraticFormula(A, B, C, u0, u1) )
	{
		if(u0>u1)
			swap(u0, u1);
		return true;
	}
	
	return false;
}

void main()
{	
	//This particle's data
	vec3 pA0 = PositionMass.xyz;
	vec3 vA0 = VelocityRadius.xyz;
	vec3 pA1 = pA0 + vA0*dt;
	vec3 vA1 = vA0 + vec3(0.0, -9.81, 0.0)*dt;
	float rA = VelocityRadius.w;
	
	vec3 Ci;
	float u0, u1;
	
	//First check collision with plane...
	if(spherePlaneSweep(rA, pA0, pA1, planeNormal, planeDistance, Ci, u0))
	{
		//calculate som collision response...
		//vA1 and pA1 might have changed???
	}
	
	//Check collision with other particles
	for(int i = 0; i < particle_count; i++) 
	{
        if(i != gl_VertexID) // && !alreadyChecked(gl_VertexID, i))
		{
			vec3 pB0 = PositionsMasses[i].xyz;
			vec3 vB0 = VelocitiesRadiuses[i].xyz;
			vec3 pB1 = pB0 + vB0*dt;
			float rB = VelocitiesRadiuses[i].w;
			
			if(sphereSphereSweep(rA, pA0, pA1, rB, pB0, pB1, u0,u1))
			{
				//calculate some collision response...
				//change vA1 and pA1 accordingly!
			}
		}
	}
	
	Out.Velocity_out = vA1;
	Out.Position_out = pA1;
}


